/*
 * StartApplication.java
 *
 * Created on October 8, 2006, 12:15 AM
 */

package org.sunspotworld;

import com.sun.spot.io.j2me.radiogram.Radiogram;
import com.sun.spot.io.j2me.radiogram.RadiogramConnection;
import com.sun.spot.peripheral.ChannelBusyException;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.resourcesharing.ResourceUnavailableException;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.resources.transducers.ITriColorLED;
import com.sun.spot.resources.transducers.LEDColor;
import com.sun.spot.resources.transducers.ITriColorLEDArray;

import java.io.*;
import java.util.Random;
import javax.microedition.io.Connector;
import org.sunspotworld.robowulf.Robowulf;

/**
 * The startApp method of this class is called by the VM to start the
 * application.
 * 
 * The manifest specifies this class as MIDlet-1, which means it will
 * be selected for execution.
 */
public class StartApplication extends Spotlet {
    private static final int TC = 27;
    private static final double SPEED_OF_SOUND = (331.5 + .6* TC)*39.3700787401575/1000000;  //magic number inches / meter
    private static final int MAX_DEPTH = 3;
    private static final String PORT = "45";
    private static final int LIGHT_THRESH = 400;
    private static final int DIST_THRESH = 6; // in inches
    private static int averageLight = 0;
    private static int distance = 30;
    private static Thread searching;
    private static Thread following;
    private static ITriColorLED[] leds = EDemoBoard.getInstance().getLEDs();
    private static RadiogramConnection broadcast;
    private static Radiogram xrg;
    private static Random random = new Random(new IEEEAddress(System.getProperty("IEEE_ADDRESS")).asLong());
    private static boolean avoiding = false;
    private static int depth = 0;
    private static IIOPin pin = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D0];
    
    /**
     * If the Spot application is running have it stop.
     * Can be defined by a subclass of Spotlet.
     */
    public void quit () { 
        if (broadcast != null) {
            try {
                broadcast.close();
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }
    
    private void reportLoop() {
        int retry;
        while(true) {
            try {
                xrg.reset();
                xrg.writeBoolean(avoiding);
                xrg.writeInt(averageLight);
                xrg.writeInt(distance);
                retry = 0;
                while (retry < 5) {
                    try {
                        broadcast.send(xrg);               // broadcast reading
                        //System.out.println("Reading sent");
                        break;
                    } catch (ChannelBusyException ex) {
                        retry++;
                        Utils.sleep(random.nextInt(10) + 2);  // wait a random amount before retrying
                    }
                }
            } catch (IOException ex) {
                ex.printStackTrace();
            }
            Utils.sleep(500);
       }
   }
        
   private static void search() {
        int i = 0;
        while(averageLight < LIGHT_THRESH && i < 60) {
            Robowulf.getInstance().left(20);
            i+=20;
            isRoom();    
        }
        Robowulf.getInstance().stop();
        i=0;
        while(averageLight < LIGHT_THRESH && i < 420) {
            Robowulf.getInstance().right(20);
            i+=20;
            isRoom();
        }
        Robowulf.getInstance().stop();
        while(averageLight < LIGHT_THRESH ) {
            isRoom();
            Utils.sleep(200);
        }
    }

    private static void follow() {
        while (averageLight > LIGHT_THRESH) {
            if (isRoom()) {
                Robowulf.getInstance().forward(1);
            } else {
                while (!isRoom() && averageLight > LIGHT_THRESH) {
                    avoiding = true;
                    avoid();
                    avoiding = false;
                }
            }
        }
        Robowulf.getInstance().stop();
    }
        
    private static Thread sensorThread = new Thread() {
        public void run() {
            while(true) {
                search();
                follow();
            }
        }
    };
    
    private static void setColor() {
        LEDColor color;
        if (distance < 1.5*DIST_THRESH) {
            color = LEDColor.RED;
        } else if(distance < 3*DIST_THRESH) {
            color = LEDColor.YELLOW;
        } else {
            color = LEDColor.GREEN;
        }
        for (int i=0; i<8; i++) {
            leds[i].setColor(color);
        }
    }
    
    private static void display() {
        System.out.println(averageLight);
        if (avoiding) {
            for (int i=0; i< 2; i++) {
                leds[i].setOn();
            }
        }else {
            for (int i=0; i<8; i++) {
                leds[7-i].setOn(i <= (averageLight/100));
            }
        }
    }
    
    public void run() {
        System.out.println("Hello, world");
        Spot.getInstance().getSleepManager().disableDeepSleep();
        Robowulf.getInstance().setUnits(Robowulf.INCHES);
        try {
            Spot.getInstance().getOTACommandServer().setSuspended(false);
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        try {
            //Robowulf.getInstance().findStops();
            //Robowulf.getInstance().testBackward();
            broadcast = (RadiogramConnection)Connector.open("radiogram://broadcast:"+PORT);
            xrg = (Radiogram)broadcast.newDatagram(broadcast.getMaximumLength());
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        new Thread(sensorThread).start();
        reportLoop();
    }
    
    private static void avoid() {
        //Robowulf.getInstance().stop();
        //*
        depth++;
        avoiding = avoiding && depth < MAX_DEPTH;
        if (avoiding) {
            Robowulf.getInstance().right(90);
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            Robowulf.getInstance().left(90);
            while (!isRoom() && avoiding) {
                Robowulf.getInstance().right(90);
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                Robowulf.getInstance().left(90);
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            Robowulf.getInstance().left(90);
            while (!isRoom() && avoiding) {
                Robowulf.getInstance().right(90);
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                if (isRoom()) {
                    Robowulf.getInstance().forward(2);
                } else {
                    avoid();
                }
                Robowulf.getInstance().left(90);
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            if (isRoom()) {
                Robowulf.getInstance().forward(2);
            } else {
                avoid();
            }
            Robowulf.getInstance().right(90);
        }
        depth--; //*/
    }
    
    private static boolean isRoom() {
        try {
            distance = (int)((Robowulf)Robowulf.getInstance()).senseForwardRange();
            setColor();
            averageLight = EDemoBoard.getInstance().getLightSensor().getAverageValue();
            //System.out.println(averageLight);
            display();
        } catch (IOException ex) {
            ex.printStackTrace();
        } catch (ResourceUnavailableException ex) {
            ex.printStackTrace();
        }
        return distance > DIST_THRESH;
    }
}
